package novemberBot.controller;

import novemberBot.FactoryFloorRobot;
import novemberBot.navigation.Navigator;
import novemberBot.navigation.Odometer;
import novemberBot.sensor.LineChecker;
import novemberBot.sensor.MotorisedUsS;
import novemberBot.sensor.UsDataResponder;
import novemberBot.utils.Trig;

/**This class contains method for performing both ultrasonic localization, which works only in corners. And for light sensor
 * localization which basically uses the position independent grid snapping
 * @see novemberBot.navigation.GridSnapper
 * @author Antoine Lennartz*/
public class Localizer extends UsDataResponder{
	
	public final int FALL_OFF_LIMIT=37;
	private final double ROTATION_SPEED = 30;
	public final double[] SENSOR_ERROR=new double[]{4, 5};
	private final int COUNT_LIMIT=2, TREND_DELAY=35;
	
	/**reference to high ultrasonic sensor*/
	private MotorisedUsS muss = FactoryFloorRobot.getInstance().getUsPoller().getUsHigh();
	
	private static enum trend{INCREASING, DECREASING};
	
	private  int data0=FALL_OFF_LIMIT, count=COUNT_LIMIT;
	
	private FactoryFloorRobot ffr;
	private Odometer odo;
	private Navigator theCaptain;
	private trend t=trend.DECREASING;
	private LineChecker Left;
	private LineChecker Right;
	
	
	/**Constructor sets up the filter*/
	public Localizer() {
		super("median2", Sensor.HIGH);
		filter.setFilterAmount(3);
		filter.newMeasurement();
		filter.setMaxD(FALL_OFF_LIMIT);
		ffr=FactoryFloorRobot.getInstance();
		odo=ffr.getOdometer();
		theCaptain=ffr.getNavigator();
		Left=ffr.getLeftChecker();
		Right=ffr.getRightChecker();
		
	}
	/**uses grid snapping to correct position*/
	public void lightLocalize(){
		double searchRange=15;
		double[] iPos = new double[3];
		odo.getPosition(iPos);
		Left.setEnable(true);
		Right.setEnable(true);
		theCaptain.useSnap(false);
		theCaptain.setForward(-searchRange/2.0, false);
		theCaptain.useSnap(true);
		theCaptain.setForward(searchRange, false);
		theCaptain.useSnap(false);
		theCaptain.setForward(-searchRange/2.0, false);
		theCaptain.setRotation(nearestTheta(iPos[2]+90), false);
		theCaptain.setForward(-searchRange/2.0, false);
		theCaptain.useSnap(true);
		theCaptain.setForward(searchRange, false);
		theCaptain.useSnap(false);
		theCaptain.setForward(-searchRange/2.0, false);
		iPos[2]=nearestTheta(iPos[2]);
		theCaptain.setTarget(Trig.nearestCross(iPos), false);
	}
	
	/**computes nearest multiple of 90 angle of the parameter
	 * @param theta -  an angle in degrees
	 * @return nearest of these angles to theta 0, 90, 180, 270*/
	private double nearestTheta(double theta){
		
		if(theta>=45 && theta<135){
			return 90;
		}else if(theta>=135 && theta<225){
			return 180;
		}else if(theta>=225 && theta<315){
			return 270;
		}else {
			return 0;
		}
	}
	
	
	/**localizes using us sensors*/
	public void usLocalize(){
		double[] pos = new double[3];
		System.out.println("pause");
		muss.pauseSweep();
		System.out.println("turn");
		muss.getSensorMotor().setAutoStop(false);
		double sOri=Trig.fixAngle2(muss.getSensorMotor().getGearTachoCount());
		
		while(sOri>1 || sOri<-1){//for some reason this was necessary
			muss.sweepTo(0, 60, false);
			sOri=Trig.fixAngle2(muss.getSensorMotor().getGearTachoCount());
		}
		
		try {Thread.sleep(200);} catch (InterruptedException e) {}

		System.out.println("need");
		needsData=true;
		// rotate the ffr until it sees no wall
		ffr.setRotationSpeed(ROTATION_SPEED);
		filter.newMeasurement();
		while (getData() <= FALL_OFF_LIMIT) Thread.yield();
		// keep rotating until the robot sees a wall, then start checking for min dist.
		while (getData() >= FALL_OFF_LIMIT) Thread.yield();	
		t=trend.DECREASING;
		while(t==trend.DECREASING)getData();	
		odo.setPosition(new double[]{0, data0+SENSOR_ERROR[1], 180+TREND_DELAY}, new boolean[]{false, true, true}); // here is prob
		while(t==trend.INCREASING)getData();
		while(t==trend.DECREASING)getData();
		odo.getPosition(pos);
		odo.setPosition(new double[]{data0+SENSOR_ERROR[0], 0, (270+TREND_DELAY+pos[2])/2}, new boolean[]{true, false, true});	
		ffr.stop();
		needsData=false;
		//reset sensors for sweeping
		muss.getSensorMotor().setAutoStop(true);
		muss.resumeSweep();
	}
	/**gets data from filter and sets the trend t*/
	private int getData(){
		int data1=filter.getData();
		if(t==trend.INCREASING){
			if(data1<data0){
				count--;
			}else if(data1>data0)count=COUNT_LIMIT;
			data0=data1;
			if(count<COUNT_LIMIT && count>0){	
				return data1;
			}else if(count==0) {
				count=COUNT_LIMIT;
				t=trend.DECREASING;
				return data1;
			}
		}else{
			if(data1>data0){
				count--;
			}else if(data1<data0)count=COUNT_LIMIT;
			data0=data1;
			if(count<COUNT_LIMIT && count>0){	
			return data1;
			}else if(count==0) {
				count=COUNT_LIMIT;
				t=trend.INCREASING;
				return data1;
			}
		}
		return data1;
	}

	/**method for LCDInfo
	 * @return string representation of trend*/
	public String trend(){
		if (t==trend.DECREASING) return "decreasing";
		else return "increasing";
	}
	/**method for LCDInfo
	 * @return string representation of data0*/
	public String data0(){
		return ""+data0;
	}


}
